/*=========================================================================
 * 
 UberSim Source Code Release
 -------------------------------------------------------------------------
 Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
 James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
 Erick Tryzelaar {erickt}@andrew.cmu.edu
 School of Computer Science, Carnegie Mellon University
 -------------------------------------------------------------------------
 This software is distributed under the GNU General Public License,
 version 2.  If you do not have a copy of this licence, visit
 www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
 Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
 in the hope that it will be useful, but WITHOUT ANY WARRANTY,
 including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 -------------------------------------------------------------------------*/

#include "mmgr.h"

#include "Collision.h"
#include "RigidBody.h"
#include "Simulator.h"
#include "Spatial.h"

/*******************************/

Simulator* Simulator::getInstance ()
{
	static Simulator instance;

	return &instance;
}

/*******************************/

Simulator::~Simulator ()
{
	shutdown ();
}

/*******************************/

void Simulator::initialize ()
{
	if (!isInitialized)
	{
		isInitialized = true;

		world = dWorldCreate ();
		dWorldSetGravity (world, 0, 0, -98.0);

		contactGroup = dJointGroupCreate (0);

		isShutdown = false;
	}
}

/*******************************/

void Simulator::shutdown ()
{
	if (!isShutdown)
	{
		isShutdown = true;

		dJointGroupDestroy (contactGroup);
		contactGroup = 0;

		dWorldDestroy (world);
		dCloseODE     ();

		isInitialized = false;
	}
}

/*******************************/

dWorldID Simulator::getWorld ()
{
	return world;
}

/*******************************/

dJointGroupID Simulator::getContactGroup ()
{
	return contactGroup;
}

/*******************************/

void Simulator::setTargetFps (Real targetFps)
{
	this->simStep = 1.0 / targetFps;
}

/*******************************/

void Simulator::beginFrame ()
{
}

/*******************************/

void Simulator::frame ()
{
	dWorldStep (world, simStep);
}

/*******************************/

void Simulator::endFrame ()
{
	dJointGroupEmpty   (contactGroup);
	collisionSet.clear ();
}

/*******************************/

bool Simulator::getIsCollisionHandled (RigidBody* rigidBody1, RigidBody* rigidBody2)
{
	typedef CollisionSet::iterator Iterator;

        std::pair <Iterator, Iterator> range1 = collisionSet.equal_range (rigidBody1);

	for (Iterator i = range1.first; i != range1.second; ++i)
	{
		if (rigidBody2 == i->second)
		{
			collisionSet.erase (i);

                        std::pair <Iterator, Iterator> range2 = collisionSet.equal_range (rigidBody2);

			for (Iterator j = range2.first; j != range2.second; ++j)
			{
				if (rigidBody2 == j->second)
				{
					collisionSet.erase (j);

					return true;
				}
			}

			return true;
		}
	}

	return false;
}

/*******************************/

void Simulator::addCollision (RigidBody* rigidBody, Collision& collision)
{
	dJointID collisionJoint = dJointCreateContact (world, contactGroup, collision.contact);
	dJointAttach (collisionJoint, 0, rigidBody->getBody ());
}

/*******************************/

void Simulator::addCollision (RigidBody* rigidBody1, RigidBody* rigidBody2, Collision& collision)
{
	dJointID collisionJoint = dJointCreateContact (world, contactGroup, collision.contact); 
	dBodyID  body1          = 0;
	dBodyID  body2          = 0;

	if (rigidBody1 && rigidBody2)
	{
		collisionSet.insert (CollisionSet::value_type (rigidBody1, rigidBody2));
		collisionSet.insert (CollisionSet::value_type (rigidBody2, rigidBody1));
	}
	
	if (rigidBody1)
	{
		body1 = rigidBody1->getBody ();
	}

	if (rigidBody2)
	{
		body2 = rigidBody2->getBody ();
	}

	dJointAttach (collisionJoint, body1, body2);
}

/*******************************/

Simulator::Simulator ()
{
	isInitialized = false;
	isShutdown    = true;

	contactGroup = 0;
}

/*******************************/
